#pragma once

#include "opencv2/opencv.hpp"
#include "opencv2\imgproc\types_c.h"
#include "opencv2/calib3d/calib3d.hpp"
#include "inputParam.h"
#include "slStruct.h"
#include "fstream"
#include "trinsic.h"
#ifndef slLog
#define slLog Log::getInstance()
#endif

using namespace cv;
using namespace SL;
namespace SL
{
    namespace calibrate
    {
        namespace
        {
            void drawCorners(std::string filename, Corners corners, Size size, std::string label)
            {
                Mat image = Mat::zeros(size, CV_8UC3);
                for (int i = 0; i < corners.size(); i++)
                {
                    auto corner = corners[i];
                    Point center(int(corner.x), int(corner.y));
                    int radius = 10;
                    Scalar lineColor(0, 0, 255);
                    int thickness = 5;
                    circle(image, center, 5, lineColor, thickness);
                    putText(image, std::to_string(i), center, FONT_HERSHEY_PLAIN, 2.0, lineColor, 1, 8, false);
                }
                imwrite(filename, image);
            }

            void drawCornersList(std::string folder, CornersList cornersList, Size size, std::string label)
            {
                for (int i = 0; i < cornersList.size(); i++)
                {
                    std::string filename = folder + "\\" + label + std::to_string(i) + ".png";
                    drawCorners(filename, cornersList[i], size, label);
                }
            }
        }

        Points getWorldCoordinates(int cornerCols, int cornerRows, float squareWidth, float squareHeight)
        {
            Points worldCoordinates;
            for (int i = 0; i < cornerRows; i++)
            {
                for (int j = 0; j < cornerCols; j++)
                {
                    worldCoordinates.push_back(Point3f((cornerCols - 1 - j) * squareWidth, (cornerRows - 1 - i) * squareHeight, 0));
                    // worldCoordinates.push_back(Point3f(j * squareWidth, i * squareHeight, 0));
                }
            }
            return worldCoordinates;
        }

        CornersList extractChessboradCorners(std::vector<Mat> &images, int cornerCols, int cornerRows, bool draw = false, std::string method = "sb")
        {
            CornersList cameraCorners;
            cameraCorners.resize(images.size());
#pragma omp parallel for ordered
            for (int i = 0; i < images.size(); i++)
            {
                slLog.log("extract chess board corners: %d", i);
                Mat image = images[i];
                Size patternSize = Size(cornerCols, cornerRows);
                std::vector<Point2f> &cameraCorner = cameraCorners[i];
                Mat grayImage;
                // cvtColor(image, grayImage, CV_BGR2GRAY);
                cv::extractChannel(image, grayImage, 0);
                // std::string filename = "D:\\code\\amiraSl\\test\\gray.png";
                // imwrite(filename, grayImage);
                // resize?
                // float resizeScale = 1024.0f / grayImage.cols;
                // resize(grayImage, grayImage, cv::Size((int)grayImage.cols * resizeScale, (int)grayImage.rows * resizeScale));

                bool found = false;
               // found = findChessboardCornersSB(grayImage, patternSize, cameraCorner, CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY);
                if (!found && method == "tradition")
                {
                    // std::cout << "use tradition" << std::endl;
                    found = findChessboardCorners(grayImage, patternSize, cameraCorner, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
                }
                if (found)
                {
                    cv::cornerSubPix(grayImage, cameraCorner, Size(11, 11), Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
                    slLog.log("Found %d corners in %d image.", cameraCorner.size(), i);
                    if (draw)
                    {

                        drawChessboardCorners(grayImage, patternSize, cameraCorner, true);
                        resize(grayImage, grayImage, Size(grayImage.cols / 3, grayImage.rows / 3));
                        imshow("Camera Calibration", grayImage);
                        waitKey(0);
                    }
                }
                else
                {
                    slLog.log("Found %d corners in %d image.", 0, i);
                }
            }
            return cameraCorners;
        }

        void cameraCalibrate(std::vector<Points> &worldCoordinates, CornersList &cornersList, Size imageSize, Intrinsic &intrinsic, int camFlags = cv::CALIB_FIX_K3)
        {
            std::vector<Mat> camRvecs, camTvecs;
            intrinsic.error = cv::calibrateCamera(worldCoordinates, cornersList, imageSize, intrinsic.matrix, intrinsic.kc, camRvecs, camTvecs, camFlags, TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON));
            Mat temp = Mat::zeros(3, 3, CV_32FC1);
            intrinsic.matrix.convertTo(temp, CV_32FC1);
            intrinsic.matrix = temp;
            intrinsic.fx = intrinsic.matrix.at<float>(0, 0);
            intrinsic.fy = intrinsic.matrix.at<float>(1, 1);
            intrinsic.tx = intrinsic.matrix.at<float>(0, 2);
            intrinsic.ty = intrinsic.matrix.at<float>(1, 2);
            slLog.log("Valid image number: %d", cornersList.size());
            slLog.log("Camera Intrinsic: fx: %f, fy: %f, tx: %f, ty: %f, error: %f", intrinsic.fx, intrinsic.fy, intrinsic.tx, intrinsic.ty, intrinsic.error);
        }

        void biocularCameraCalibrate(std::vector<Points> worldCorrdinates, CornersList leftCornersList, CornersList rightCornersList, Size imageSize, BiocularCalibrateTrinsics &biocularCalibrateTrinsics)
        {
            auto &leftIntrinsic = biocularCalibrateTrinsics.leftIntrinsic;
            auto &rightIntrinsic = biocularCalibrateTrinsics.rightIntrinsic;
            Extrinsic &extrinsic = biocularCalibrateTrinsics.extrinsic;
            Mat perViewError;
            Mat translation; // a shit bug!!!!
            double error = cv::stereoCalibrate(worldCorrdinates, rightCornersList, leftCornersList, rightIntrinsic.matrix, rightIntrinsic.kc, leftIntrinsic.matrix, leftIntrinsic.kc, imageSize, extrinsic.rotation, translation, extrinsic.essential, extrinsic.fundamental, perViewError, CALIB_FIX_INTRINSIC, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 150, DBL_EPSILON));
            std::cout << perViewError << std::endl;
            extrinsic.translation[0] = (float)translation.at<double>(0);
            extrinsic.translation[1] = (float)translation.at<double>(1);
            extrinsic.translation[2] = (float)translation.at<double>(2);
            leftIntrinsic.matrix.convertTo(leftIntrinsic.matrix, CV_32FC1);
            rightIntrinsic.matrix.convertTo(rightIntrinsic.matrix, CV_32FC1);
            extrinsic.matrix = Mat::zeros(4, 4, CV_32FC1);
            extrinsic.rotation(Range(0, 3), Range(0, 3)).copyTo(extrinsic.matrix.colRange(0, 3).rowRange(0, 3));
            extrinsic.matrix.at<float>(0, 3) = (float)extrinsic.translation[0];
            extrinsic.matrix.at<float>(1, 3) = (float)extrinsic.translation[1];
            extrinsic.matrix.at<float>(2, 3) = (float)extrinsic.translation[2];
            extrinsic.matrix.at<float>(3, 3) = 1.0f;
            for (int i = 0; i < perViewError.rows; i++)
            {
                slLog.log("Perview Error %d: %f , %f", i + 1, perViewError.at<double>(i, 0), perViewError.at<double>(i, 1));
            }
            slLog.log("Error: %f", error);
            slLog.log("Extrisnic translation: %f, %f, %f", extrinsic.translation[0], extrinsic.translation[1], extrinsic.translation[2]);
        }

        void projectorStereoCalibrate(std::vector<Points> worldCorrdinates, CornersList cameraCornersList, CornersList projectorCornersList, Size imageSize, ProjectorCalibrateTrinsics &projectorCalibrateTrinsics)
        {
            auto &cameraIntrinsic = projectorCalibrateTrinsics.cameraIntrinsic;
            auto &projectorIntrinsic = projectorCalibrateTrinsics.projectorIntrinsic;
            Extrinsic &extrinsic = projectorCalibrateTrinsics.extrinsic;

            double error = cv::stereoCalibrate(worldCorrdinates, cameraCornersList, projectorCornersList, cameraIntrinsic.matrix, cameraIntrinsic.kc, projectorIntrinsic.matrix, projectorIntrinsic.kc, imageSize, extrinsic.rotation, extrinsic.translation, extrinsic.essential, extrinsic.fundamental, CALIB_FIX_INTRINSIC + CALIB_FIX_K3, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 150, DBL_EPSILON));
            cameraIntrinsic.matrix.convertTo(cameraIntrinsic.matrix, CV_32FC1);
            projectorIntrinsic.matrix.convertTo(projectorIntrinsic.matrix, CV_32FC1);
            extrinsic.matrix = Mat::zeros(4, 4, CV_32FC1);
            extrinsic.rotation(Range(0, 3), Range(0, 3)).copyTo(extrinsic.matrix.colRange(0, 3).rowRange(0, 3));
            extrinsic.matrix.at<float>(0, 3) = (float)extrinsic.translation[0];
            extrinsic.matrix.at<float>(1, 3) = (float)extrinsic.translation[1];
            extrinsic.matrix.at<float>(2, 3) = (float)extrinsic.translation[2];
            extrinsic.matrix.at<float>(3, 3) = 1.0f;
            slLog.log("Error: %f", error);

            slLog.log("Extrisnic translation: %f, %f, %f", extrinsic.translation[0], extrinsic.translation[1], extrinsic.translation[2]);
        }

        void calibrate(CornersList cameraCornersList, Images decodeImages, Size imageSize, Size projectorSize, int cornerCols, int cornerRows, float squareWidth, float squareHeight, int windowSize, std::string savePath)
        {
            std::vector<Points> activeWorldCoordinates;
            CornersList activeImagesPoints, activeProjectImagesPoints;
            Points worldCoordinatesCache = getWorldCoordinates(cornerCols, cornerRows, squareWidth, squareHeight);
            for (int i = 0; i < cameraCornersList.size(); i++)
            {
                auto &corners = cameraCornersList[i];
                Points activeWorldCoordinate;
                Corners activeImagePoints, activeProjectImagePoints;
                for (int j = 0; j < corners.size(); j++)
                {
                    Point2f p = corners[j];
                    if (p.x > windowSize && p.y > windowSize && p.x + windowSize < imageSize.width && p.y + windowSize < imageSize.height)
                    {
                        Point2f q;
                        std::vector<Point2f> imagePoints, projectorPoints;
                        for (int h = (int)p.y - windowSize; h < p.y + windowSize; h++)
                        {
                            const Vec2f *row = decodeImages[i].ptr<Vec2f>(h);
                            for (int w = (int)p.x - windowSize; w < (int)p.x + windowSize; w++)
                            {
                                const float &x = row[w][0];
                                const float &y = row[w][1];
                                if (isnan(x) || isnan(y))
                                {
                                    continue;
                                }
                                imagePoints.push_back(Point2f((float)w, (float)h));
                                projectorPoints.push_back(Point2f((float)x, (float)y));
                            }
                        }
                        if (imagePoints.size() > 0)
                        {
                            Mat H = findHomography(imagePoints, projectorPoints, RANSAC);
                            Point3d Q = Point3d(Mat(H * Mat(Point3d(p.x, p.y, 1.0))));
                            q = Point2f((float)Q.x / (float)Q.z, (float)Q.y / (float)Q.z);
                            activeImagePoints.push_back(p);
                            activeProjectImagePoints.push_back(q);
                            activeWorldCoordinate.push_back(worldCoordinatesCache[j]);
                        }
                    }
                }
                activeWorldCoordinates.push_back(activeWorldCoordinate);
                activeImagesPoints.push_back(activeImagePoints);
                activeProjectImagesPoints.push_back(activeProjectImagePoints);
            }

            // intrinsic
            ProjectorCalibrateTrinsics projectorCalibrateTrinsics;
            Intrinsic &cameraIntrinsic = projectorCalibrateTrinsics.cameraIntrinsic;
            Intrinsic &projectorIntrinsic = projectorCalibrateTrinsics.projectorIntrinsic;
            Extrinsic &extrinsic = projectorCalibrateTrinsics.extrinsic;
            cameraCalibrate(activeWorldCoordinates, activeProjectImagesPoints, projectorSize, projectorIntrinsic, cv::CALIB_FIX_K3);
            cameraCalibrate(activeWorldCoordinates, activeImagesPoints, imageSize, cameraIntrinsic);
            projectorStereoCalibrate(activeWorldCoordinates, activeImagesPoints, activeProjectImagesPoints, imageSize, projectorCalibrateTrinsics);
            writeTrinsic(savePath, projectorCalibrateTrinsics);
            if (true)
            {
                std::string cornersPath = "D:\\code\\amiraSl\\test\\corners";
                drawCornersList(cornersPath, activeImagesPoints, imageSize, "camera");
                drawCornersList(cornersPath, activeProjectImagesPoints, projectorSize, "projector");

                std::string filename = cornersPath + "\\corners.txt";
                std::ofstream output(filename);
                output << "world, image, project" << std::endl;
                for (int i = 0; i < activeWorldCoordinates.size(); i++)
                {
                    auto worldCoordinates = activeWorldCoordinates[i];
                    output << "Image " << i << std::endl;
                    for (int j = 0; j < worldCoordinates.size(); j++)
                    {
                        auto worldCoordinate = worldCoordinates[j];
                        auto imagePoint = activeImagesPoints[i][j];
                        auto projectPoint = activeProjectImagesPoints[i][j];
                        output << "(" << worldCoordinate.x << ", " << worldCoordinate.y << ", " << worldCoordinate.z << "),   "
                               << "(" << imagePoint.x << ", " << imagePoint.y << "),  "
                               << "(" << projectPoint.x << ", " << projectPoint.y << ") " << std::endl;
                    }
                }
                output.close();
            }
        }
    }
}